﻿#include "mingzhimotor.h"
#include <QDebug>

MingzhiMotor::MingzhiMotor( int pulse_per_round, double length_per_pulse, int left_direction, int right_direction, long acce, long dece ) {
    this->pulse_per_round                        = pulse_per_round;
    this->length_per_pulse                       = length_per_pulse;
    wheel_direction[ NODE_INDEX::LEFT_NODE_ID ]  = static_cast< char >( left_direction );
    wheel_direction[ NODE_INDEX::RIGHT_NODE_ID ] = static_cast< char >( right_direction );
    this->acce                                   = acce;
    this->dece                                   = dece;
}

MingzhiMotor::~MingzhiMotor() {}

/**
初始化流程:
1、电机失能；
2、启动驱动；
3、电机使能；
4、设置速度模式；
5、运动参数；
6、设置看门狗
6、启动运动
*/
void MingzhiMotor::DataFramesMotorInit( CanDataFrame* frames, uint8& i ) {
    // CanDataFrameOperationalMode( frames[ i++ ] );
    CanDataFrameNMTMode( frames[ i++ ], NMT_MODE::NMT_OPERATION_MODE );

    CanDataFrameControlWord( frames[ i++ ], NODE_INDEX::LEFT_NODE_ID, PDS_STA::QUICK_STOP );
    CanDataFrameControlWord( frames[ i++ ], NODE_INDEX::RIGHT_NODE_ID, PDS_STA::QUICK_STOP );

    CanDataFrameControlWord( frames[ i++ ], NODE_INDEX::LEFT_NODE_ID, PDS_STA::SWITCH_ON );
    CanDataFrameControlWord( frames[ i++ ], NODE_INDEX::RIGHT_NODE_ID, PDS_STA::SWITCH_ON );

    CanDataFrameControlWord( frames[ i++ ], NODE_INDEX::LEFT_NODE_ID, PDS_STA::DISABLE_OPERATION );
    CanDataFrameControlWord( frames[ i++ ], NODE_INDEX::RIGHT_NODE_ID, PDS_STA::DISABLE_OPERATION );

    CanDataFrameControlWord( frames[ i++ ], NODE_INDEX::LEFT_NODE_ID, PDS_STA::ENABLE_OPERATION );
    CanDataFrameControlWord( frames[ i++ ], NODE_INDEX::RIGHT_NODE_ID, PDS_STA::ENABLE_OPERATION );

    //    CanDataFrameDisableMotor( frames[ i++ ], NODE_INDEX::LEFT_NODE_ID );
    //    CanDataFrameDisableMotor( frames[ i++ ], NODE_INDEX::RIGHT_NODE_ID );
    //    CanDataFrameStartDriver( frames[ i++ ], NODE_INDEX::LEFT_NODE_ID );
    //    CanDataFrameStartDriver( frames[ i++ ], NODE_INDEX::RIGHT_NODE_ID );
    //    CanDataFrameEnableMotor( frames[ i++ ], NODE_INDEX::LEFT_NODE_ID );
    //    CanDataFrameEnableMotor( frames[ i++ ], NODE_INDEX::RIGHT_NODE_ID );

    CanDataFrameSpeedMode( frames[ i++ ], NODE_INDEX::LEFT_NODE_ID );
    CanDataFrameSpeedMode( frames[ i++ ], NODE_INDEX::RIGHT_NODE_ID );

    CanDataFrameSetSpeedValue( frames[ i++ ], NODE_INDEX::LEFT_NODE_ID, 0 );
    CanDataFrameSetSpeedValue( frames[ i++ ], NODE_INDEX::RIGHT_NODE_ID, 0 );
    CanDataFrameSetAcce( frames[ i++ ], NODE_INDEX::LEFT_NODE_ID, acce );
    CanDataFrameSetAcce( frames[ i++ ], NODE_INDEX::RIGHT_NODE_ID, acce );
    CanDataFrameSetDece( frames[ i++ ], NODE_INDEX::LEFT_NODE_ID, dece );
    CanDataFrameSetDece( frames[ i++ ], NODE_INDEX::RIGHT_NODE_ID, dece );

    //看门狗功能配置（节点保护）
    CanDataFrameWatchDogStop( frames[ i++ ], NODE_INDEX::LEFT_NODE_ID );
    CanDataFrameWatchDogStop( frames[ i++ ], NODE_INDEX::RIGHT_NODE_ID );
    CanDataFrameWatchDogSetMonitorTimer( frames[ i++ ], NODE_INDEX::LEFT_NODE_ID );                   // 1、设置监控时间
    CanDataFrameWatchDogSetMonitorTimer( frames[ i++ ], NODE_INDEX::RIGHT_NODE_ID );                  // 1、设置监控时间
    CanDataFrameWatchDogSetDepartureEvent( frames[ i++ ], NODE_INDEX::LEFT_NODE_ID );                 // 2、设置触发事件，以SYNC为触发条件
    CanDataFrameWatchDogSetDepartureEvent( frames[ i++ ], NODE_INDEX::RIGHT_NODE_ID );                // 2、设置触发事件，以SYNC为触发条件
    CanDataFrameWatchDogSetMotorActionAfterInterruption( frames[ i++ ], NODE_INDEX::LEFT_NODE_ID );   // 3、设置通讯中断后电机动作
    CanDataFrameWatchDogSetMotorActionAfterInterruption( frames[ i++ ], NODE_INDEX::RIGHT_NODE_ID );  // 3、设置通讯中断后电机动作
    CanDataFrameWatchDogStart( frames[ i++ ], NODE_INDEX::LEFT_NODE_ID );                             // 4、启动看门狗
    CanDataFrameWatchDogStart( frames[ i++ ], NODE_INDEX::RIGHT_NODE_ID );                            // 4、启动看门狗

    // CanDataFrameStartMove(frames[i++], NODE_INDEX::LEFT_NODE_ID);     // 启动运动
    // CanDataFrameStartMove(frames[i++], NODE_INDEX::RIGHT_NODE_ID);    // 启动运动
    CanDataFrameEnableMotor( frames[ i++ ], NODE_INDEX::LEFT_NODE_ID );
    CanDataFrameEnableMotor( frames[ i++ ], NODE_INDEX::RIGHT_NODE_ID );
}

/**
 * speedl/speedr:cm/s
*/
void MingzhiMotor::DataFramesMotorSpeed( CanDataFrame* frames, uint8& i, double speedl, double speedr ) {
    /*设置速度 公式说明：
     * “right_direction_” 因为电机安装位置反转，用来修正设定方向为同符号
     * “10 * rightSpeed” 设定左轮速度在最高速度，单位 mm/s
     * Param::length_per_pulse 每个脉冲的距离 单位：mm/pulse
     * 乘10 ：驱动器的解析度为0.1 pulse ，驱动器解析值为整数。
     */

    long speedL = static_cast< long >( wheel_direction[ NODE_INDEX::LEFT_NODE_ID ] * 10 * speedl / length_per_pulse );
    long speedR = static_cast< long >( wheel_direction[ NODE_INDEX::RIGHT_NODE_ID ] * 10 * speedr / length_per_pulse );

    CanDataFrameSetSpeedValue( frames[ i++ ], NODE_INDEX::LEFT_NODE_ID, speedL );
    CanDataFrameSetSpeedValue( frames[ i++ ], NODE_INDEX::RIGHT_NODE_ID, speedR );
}

void MingzhiMotor::DataFramesMotorRequest( CanDataFrame* frames, uint8& i ) {
    CanDataFrameSYNC( frames[ i++ ] );
    CanDataFrameRquestMotorCurSpeed( frames[ i++ ], NODE_INDEX::LEFT_NODE_ID );
    CanDataFrameRquestMotorCurSpeed( frames[ i++ ], NODE_INDEX::RIGHT_NODE_ID );
    CanDataFrameRquestMotorPosition( frames[ i++ ], NODE_INDEX::LEFT_NODE_ID );
    CanDataFrameRquestMotorPosition( frames[ i++ ], NODE_INDEX::RIGHT_NODE_ID );
    CanDataFrameRquestMotorFaultCode( frames[ i++ ], NODE_INDEX::LEFT_NODE_ID );
    CanDataFrameRquestMotorFaultCode( frames[ i++ ], NODE_INDEX::RIGHT_NODE_ID );
}

QString MingzhiMotor::RecDataParse( CanDataFrame* frames, uint8 cnt ) {
    QString msg;

    if ( nullptr == frames || 0 >= cnt ) return msg;

    uint32        uID;
    CanDataFrame* frame;
    for ( int i = 0; i < cnt; i++ ) {
        frame = &( frames[ i ] );
        uID   = frame->uID;
        if ( uID == ( NODE_INDEX::LEFT_NODE_ID + CAN_ID::CAN_ID_REC ) )
            msg = DataParse( NODE_INDEX::LEFT_NODE_ID, frame->arryData, mfb_para.lmfp );
        else if ( uID == ( NODE_INDEX::RIGHT_NODE_ID + CAN_ID::CAN_ID_REC ) )
            msg = DataParse( NODE_INDEX::RIGHT_NODE_ID, frame->arryData, mfb_para.rmfp );
    }
    return msg;
}

DMotorFeedBackPara MingzhiMotor::RetMotorFeedbackParam() {
    return mfb_para;
}

void MingzhiMotor::DataFramesMotorEnable( CanDataFrame* frames, uint8& i ) {
    CanDataFrameEnableMotor( frames[ i++ ], NODE_INDEX::LEFT_NODE_ID );
    CanDataFrameEnableMotor( frames[ i++ ], NODE_INDEX::RIGHT_NODE_ID );
}

void MingzhiMotor::DataFramesMotorDisable( CanDataFrame* frames, uint8& i ) {
    CanDataFrameDisableMotor( frames[ i++ ], NODE_INDEX::LEFT_NODE_ID );
    CanDataFrameDisableMotor( frames[ i++ ], NODE_INDEX::RIGHT_NODE_ID );
}

//全部伺服电机设置停止模式
void MingzhiMotor::DataFramesStopMotor( CanDataFrame* frames, uint8& i ) {
    DataFramesMotorSpeed( frames, i, 0, 0 );
    CanDataFrameWatchDogStop( frames[ i++ ], NODE_INDEX::LEFT_NODE_ID );
    CanDataFrameWatchDogStop( frames[ i++ ], NODE_INDEX::RIGHT_NODE_ID );
    CanDataFrameControlWord( frames[ i++ ], NODE_INDEX::LEFT_NODE_ID, PDS_STA::QUICK_STOP );
    CanDataFrameControlWord( frames[ i++ ], NODE_INDEX::RIGHT_NODE_ID, PDS_STA::QUICK_STOP );
    CanDataFrameNMTMode( frames[ i++ ], NMT_MODE::NMT_STOP_MODE );
}

void MingzhiMotor::DataFramesAccDecUpdate( CanDataFrame* frames, uint8& i, double& acc, double& dec ) {
    long m_acce = static_cast< long >( acc * pulse_per_round );
    long m_dece = static_cast< long >( dec * pulse_per_round );
    if ( m_acce == acce && m_dece == dece ) { return; }  //同样的配置，不重复执行
    acce = m_acce;
    dece = m_dece;

    CanDataFrameSetAcce( frames[ i++ ], NODE_INDEX::LEFT_NODE_ID, acce );   //伺服电机设置加速度
    CanDataFrameSetAcce( frames[ i++ ], NODE_INDEX::RIGHT_NODE_ID, acce );  //伺服电机设置加速度
    CanDataFrameSetDece( frames[ i++ ], NODE_INDEX::LEFT_NODE_ID, dece );   //伺服电机设置减速度
    CanDataFrameSetDece( frames[ i++ ], NODE_INDEX::RIGHT_NODE_ID, dece );  //伺服电机设置减速度
}

void MingzhiMotor::DataFramesGetDriveErrInfo( CanDataFrame* frames, uint8& i ) {
    // CanDataFrameGetErrInfo( frames[ i++ ], left_drive_id );
    // CanDataFrameGetErrInfo( frames[ i++ ], right_drive_id );
    CanDataFrameRquestMotorFaultCode( frames[ i++ ], NODE_INDEX::LEFT_NODE_ID );   // 获取故障信息
    CanDataFrameRquestMotorFaultCode( frames[ i++ ], NODE_INDEX::RIGHT_NODE_ID );  // 获取故障信息
}

void MingzhiMotor::ResetWatchDog( CanDataFrame* frames, uint8& i ) {
    CanDataFrameWatchDogStop( frames[ i++ ], NODE_INDEX::LEFT_NODE_ID );
    CanDataFrameWatchDogStop( frames[ i++ ], NODE_INDEX::RIGHT_NODE_ID );
    CanDataFrameWatchDogStart( frames[ i++ ], NODE_INDEX::LEFT_NODE_ID );
    CanDataFrameWatchDogStart( frames[ i++ ], NODE_INDEX::RIGHT_NODE_ID );
}

//报警清除
void MingzhiMotor::AlarmClean( CanDataFrame* frames, uint8& i ) {
    CanDataFrameAlarmClean( frames[ i++ ], NODE_INDEX::LEFT_NODE_ID, ALARM_CLEAR_ACTION::ACAF );
    CanDataFrameAlarmClean( frames[ i++ ], NODE_INDEX::LEFT_NODE_ID, ALARM_CLEAR_ACTION::ACAT );
    CanDataFrameAlarmClean( frames[ i++ ], NODE_INDEX::RIGHT_NODE_ID, ALARM_CLEAR_ACTION::ACAF );
    CanDataFrameAlarmClean( frames[ i++ ], NODE_INDEX::RIGHT_NODE_ID, ALARM_CLEAR_ACTION::ACAT );
}

#define MSG_L "错误 左轮："
#define MSG_R "错误 右轮："
#define QSTRING_HEX_SHOW( num ) ( QString( "%1" ).arg( num, 4, 16, QLatin1Char( '0' ) ).toUpper() )

QString MingzhiMotor::MotorErrInfo( NODE_INDEX index, int err_info ) {
    QString msg;
    if ( NODE_INDEX::LEFT_NODE_ID == index )
        msg = MSG_L;
    else if ( NODE_INDEX::RIGHT_NODE_ID == index )
        msg = MSG_R;

    switch ( err_info ) {
    case ERR_CODE::ERR00:
        break;
    case ERR_CODE::ERR01:
        msg += "驱动器过流，错误码：0x" + QSTRING_HEX_SHOW( err_info );

        break;
    case ERR_CODE::ERR02:
        msg += "驱动器过压，错误码：0x" + QSTRING_HEX_SHOW( err_info );
        break;
    case ERR_CODE::ERR03:
        msg += "驱动器处理器过温，错误码：0x" + QSTRING_HEX_SHOW( err_info );
        break;
    case ERR_CODE::ERR04:
        msg += "预留功能，错误码：0x" + QSTRING_HEX_SHOW( err_info );
        break;
    case ERR_CODE::ERR05:
        msg += "驱动器内部电压错误，错误码：0x" + QSTRING_HEX_SHOW( err_info );
        break;
    case ERR_CODE::ERR06:
        msg += "位置误差超限，错误码：0x" + QSTRING_HEX_SHOW( err_info );
        break;
    case ERR_CODE::ERR07:
        msg += "电机编码器未连接，错误码：0x" + QSTRING_HEX_SHOW( err_info );
        break;
    case ERR_CODE::ERR08:
        msg += "全闭环混合偏差超限，错误码：0x" + QSTRING_HEX_SHOW( err_info );
        break;
    case ERR_CODE::ERR09:
        msg += "第二编码器未连接，错误码：0x" + QSTRING_HEX_SHOW( err_info );
        break;
    case ERR_CODE::ERR10:
        msg += "再生电势泄放失败，错误码：0x" + QSTRING_HEX_SHOW( err_info );
        break;
    case ERR_CODE::ERR11:
        msg += "安全转矩禁止中（STO），错误码：0x" + QSTRING_HEX_SHOW( err_info );
        break;
    case ERR_CODE::ERR12:
        msg += "预留功能，错误码：0x" + QSTRING_HEX_SHOW( err_info );
        break;
    case ERR_CODE::ERR13:
        msg += "FPGA错误，错误码：0x" + QSTRING_HEX_SHOW( err_info );
        break;
    case ERR_CODE::ERR14:
        msg += "参数读取失败，错误码：0x" + QSTRING_HEX_SHOW( err_info );
        break;
    case ERR_CODE::ERR15:
        msg += "绝对值编码器多圈错误，错误码：0x" + QSTRING_HEX_SHOW( err_info );
        break;
    case ERR_CODE::ERR16:
        msg += "电机堵转保护，错误码：0x" + QSTRING_HEX_SHOW( err_info );
        break;
    case ERR_CODE::ERR17:
        msg += "驱动器功率模块过温，错误码：0x" + QSTRING_HEX_SHOW( err_info );
        break;
    case ERR_CODE::ERR18:
        msg += "正转禁止限位，错误码：0x" + QSTRING_HEX_SHOW( err_info );
        break;
    case ERR_CODE::ERR19:
        msg += "反转禁止限位，错误码：0x" + QSTRING_HEX_SHOW( err_info );
        break;
    case ERR_CODE::ERR20:
        msg += "正转禁止限位及反转禁止限位，错误码：0x" + QSTRING_HEX_SHOW( err_info );
        break;
    case ERR_CODE::ERR21:
        msg += "电机重载状态，错误码：0x" + QSTRING_HEX_SHOW( err_info );
        break;
    case ERR_CODE::ERR22:
        msg += "在电机未使能时命令其运转，错误码：0x" + QSTRING_HEX_SHOW( err_info );
        break;
    case ERR_CODE::ERR23:
        msg += "驱动器低压，错误码：0x" + QSTRING_HEX_SHOW( err_info );
        break;
    case ERR_CODE::ERR24:
        msg += "调用的Q程序段为空，错误码：0x" + QSTRING_HEX_SHOW( err_info );
        break;
    case ERR_CODE::ERR25:
        msg += "电机速度超过限值，错误码：0x" + QSTRING_HEX_SHOW( err_info );
        break;
    case ERR_CODE::ERR26:
        msg += "驱动器主回路电源输入缺相，错误码：0x" + QSTRING_HEX_SHOW( err_info );
        break;
    case ERR_CODE::ERR27:
        msg += "数字量输入紧急停止，错误码：0x" + QSTRING_HEX_SHOW( err_info );
        break;
    case ERR_CODE::ERR28:
        msg += "绝对值编码器欠压，错误码：0x" + QSTRING_HEX_SHOW( err_info );
        break;
    case ERR_CODE::ERR29:
        msg += "绝对位置丢失，错误码：0x" + QSTRING_HEX_SHOW( err_info );
        break;
    case ERR_CODE::ERR30:
        msg += "绝对位置溢出，错误码：0x" + QSTRING_HEX_SHOW( err_info );
        break;
    case ERR_CODE::ERR31:
        msg += "电机过温，错误码：0x" + QSTRING_HEX_SHOW( err_info );
        break;
    case ERR_CODE::ERR32:
        msg += "驱动器欠压，错误码：0x" + QSTRING_HEX_SHOW( err_info );
        break;
    case ERR_CODE::ERR33:
        msg += "参数保存失败，错误码：0x" + QSTRING_HEX_SHOW( err_info );
        break;
    case ERR_CODE::ERR34:
        msg += "I/O信号功能复用，错误码：0x" + QSTRING_HEX_SHOW( err_info );
        break;
    case ERR_CODE::ERR35:
        msg += "总线看门狗触发，错误码：0x" + QSTRING_HEX_SHOW( err_info );
        break;
    case ERR_CODE::ERR36:
        msg += "编码器报错，待确定，错误码：0x" + QSTRING_HEX_SHOW( err_info );
        break;
    case ERR_CODE::ERR37:
        msg += "其他错误，错误码：0x" + QSTRING_HEX_SHOW( err_info );
        break;
    default:
        msg += "驱动未定义错误，错误码：0x" + QSTRING_HEX_SHOW( err_info );
        break;
    }

    if ( 0 == msg.compare( QString::fromLocal8Bit( MSG_R ) ) || 0 == msg.compare( QString::fromLocal8Bit( MSG_L ) ) ) msg = "";
    return msg;
}
//对外接口

QString MingzhiMotor::DataParse( NODE_INDEX index, uint8 ( &array )[ 8 ], SMotorFeedBackPara& para ) {
    DATA_CONV dc;
    QString   msg;

    std::copy( std::begin( array ), std::end( array ), std::begin( dc.array ) );
    if ( CS::CS_RESPOND_ERR == dc.val.cs ) return msg;

    //根据协议解析
    switch ( dc.val.od_index ) {
    case DO_INDEX::DO_READ_SPEED:
        para.speed = static_cast< double >( static_cast< int >( dc.val.val ) * length_per_pulse * 0.1 * 0.01 * wheel_direction[ index ] );
        break;
    case DO_INDEX::DO_READ_POS:
        para.pos = static_cast< int >( dc.val.val ) * wheel_direction[ index ];
        break;
    case DO_INDEX::DO_READ_ERR:
        para.err = static_cast< int >( dc.val.val );
        msg      = MotorErrInfo( index, para.err );
        break;
    default:
        break;
    }
    return msg;
}
// long ls = static_cast< long >( wheel_direction[NODE_INDEX::LEFT_NODE_ID] * 10 * speedl / length_per_pulse ) * 10;

//节点进入NMT operation模式
void MingzhiMotor::CanDataFrameNMTMode( CanDataFrame& frame, NMT_MODE mode ) {
    frame.uID           = 0;
    frame.nSendType     = 0;
    frame.bRemoteFlag   = 0;
    frame.bExternFlag   = 0;
    frame.nDataLen      = 2;
    frame.arryData[ 0 ] = mode;
    frame.arryData[ 1 ] = 0;
}

// 电机使能
void MingzhiMotor::CanDataFrameEnableMotor( CanDataFrame& frame, uint8 drive_id ) {
    frame.uID         = CAN_ID::CAN_ID_SEND + drive_id;
    frame.nSendType   = 0;
    frame.bRemoteFlag = 0;
    frame.bExternFlag = 0;
    frame.nDataLen    = 8;
    for ( int i = 0; i < 8; i++ ) {
        frame.arryData[ i ] = enable_motor[ i ];
    }
}

// 电机失能
void MingzhiMotor::CanDataFrameDisableMotor( CanDataFrame& frame, uint8 drive_id ) {
    frame.uID         = CAN_ID::CAN_ID_SEND + drive_id;
    frame.nSendType   = 0;
    frame.bRemoteFlag = 0;
    frame.bExternFlag = 0;
    frame.nDataLen    = 8;
    for ( int i = 0; i < 8; i++ ) {
        frame.arryData[ i ] = disable_motor[ i ];
    }
}

// 启动驱动
void MingzhiMotor::CanDataFrameStartDriver( CanDataFrame& frame, uint8 drive_id ) {
    frame.uID         = CAN_ID::CAN_ID_SEND + drive_id;
    frame.nSendType   = 0;
    frame.bRemoteFlag = 0;
    frame.bExternFlag = 0;
    frame.nDataLen    = 8;
    for ( int i = 0; i < 8; i++ ) {
        frame.arryData[ i ] = start_driver[ i ];
    }
}

// // 启动运动
// void MingzhiMotor::CanDataFrameStartMove(CanDataFrame& frame, uint8 drive_id)
// {
//     frame.uID         = CAN_ID::CAN_ID_SEND + drive_id;
//     frame.nSendType   = 0;
//     frame.bRemoteFlag = 0;
//     frame.bExternFlag = 0;
//     frame.nDataLen    = 8;
//     for ( int i = 0; i < 8; i++ ) {
//         frame.arryData[i] = start_motor[i];
//     }
// }

// // 停止运动
// void MingzhiMotor::CanDataFrameStopMove(CanDataFrame& frame, uint8 drive_id)
// {
//     frame.uID         = CAN_ID::CAN_ID_SEND + drive_id;
//     frame.nSendType   = 0;
//     frame.bRemoteFlag = 0;
//     frame.bExternFlag = 0;
//     frame.nDataLen    = 8;
//     for ( int i = 0; i < 8; i++ ) {
//         frame.arryData[i] = stop_motor[i];
//     }
// }

// 伺服电机状态设置
void MingzhiMotor::CanDataFrameControlWord( CanDataFrame& frame, NODE_INDEX index, PDS_STA mode ) {
    DATA_ENC de;
    de.cs          = CS::CS_WRITE_2;
    de.od_index    = DO_INDEX::DO_CONTROL_BYTE;
    de.od_subindex = 0;
    de.val         = mode;
    DATA_CONV dc;
    dc.val = de;

    frame.uID         = CAN_ID::CAN_ID_SEND + index;
    frame.nSendType   = 0;
    frame.bRemoteFlag = 0;
    frame.bExternFlag = 0;
    frame.nDataLen    = 8;
    for ( int i = 0; i < 8; i++ ) {
        frame.arryData[ i ] = dc.array[ i ];
    }
}

// 伺服电机设置速度模式
void MingzhiMotor::CanDataFrameSpeedMode( CanDataFrame& frame, uint8 drive_id ) {
    frame.uID         = CAN_ID::CAN_ID_SEND + drive_id;
    frame.nSendType   = 0;
    frame.bRemoteFlag = 0;
    frame.bExternFlag = 0;
    frame.nDataLen    = 8;
    for ( int i = 0; i < 8; i++ ) {
        frame.arryData[ i ] = speed_mode[ i ];
    }
}

//看门狗功能配置（节点保护）
// 1、设置监控时间
void MingzhiMotor::CanDataFrameWatchDogSetMonitorTimer( CanDataFrame& frame, uint8 drive_id ) {
    // frame.uID         = CAN_ID::CAN_ID_SEND + drive_id;
    // frame.nSendType   = 0;
    // frame.bRemoteFlag = 0;
    // frame.bExternFlag = 0;
    // frame.nDataLen    = 8;
    // for ( int i = 0; i < 8; i++ ) {
    //     frame.arryData[i] = watchdog_monitor_time[i];
    // }

    DATA_ENC de;
    de.cs          = CS::CS_WRITE_2;
    de.od_index    = DO_INDEX::DO_WATCH_DOG;
    de.od_subindex = WD_SUB_INDEX::MonitorTime;
    de.val         = MONITOR_TIMER;
    DATA_CONV dc;
    dc.val = de;

    frame.uID         = CAN_ID::CAN_ID_SEND + drive_id;
    frame.nSendType   = 0;
    frame.bRemoteFlag = 0;
    frame.bExternFlag = 0;
    frame.nDataLen    = 8;
    for ( int i = 0; i < 8; i++ ) {
        frame.arryData[ i ] = dc.array[ i ];
    }
}

// 2、设置触发事件，以SYNC为触发条件
void MingzhiMotor::CanDataFrameWatchDogSetDepartureEvent( CanDataFrame& frame, uint8 drive_id ) {
    // frame.uID         = CAN_ID::CAN_ID_SEND + drive_id;
    // frame.nSendType   = 0;
    // frame.bRemoteFlag = 0;
    // frame.bExternFlag = 0;
    // frame.nDataLen    = 8;
    // for ( int i = 0; i < 8; i++ ) {
    //     frame.arryData[i] = watchdog_departureEvent[i];
    // }

    DATA_ENC de;
    de.cs          = CS::CS_WRITE_2;
    de.od_index    = DO_INDEX::DO_WATCH_DOG;
    de.od_subindex = WD_SUB_INDEX::DepartureEvent;
    de.val         = WD_DEPARTURE_EVENT::SYNC;
    DATA_CONV dc;
    dc.val = de;

    frame.uID         = CAN_ID::CAN_ID_SEND + drive_id;
    frame.nSendType   = 0;
    frame.bRemoteFlag = 0;
    frame.bExternFlag = 0;
    frame.nDataLen    = 8;
    for ( int i = 0; i < 8; i++ ) {
        frame.arryData[ i ] = dc.array[ i ];
    }
}

// 3、设置通讯中断后电机动作
void MingzhiMotor::CanDataFrameWatchDogSetMotorActionAfterInterruption( CanDataFrame& frame, uint8 drive_id ) {
    // frame.uID         = CAN_ID::CAN_ID_SEND + drive_id;
    // frame.nSendType   = 0;
    // frame.bRemoteFlag = 0;
    // frame.bExternFlag = 0;
    // frame.nDataLen    = 8;
    // for ( int i = 0; i < 8; i++ ) {
    //     frame.arryData[i] = watchdog_motorActionAfterInterruption[i];
    // }

    DATA_ENC de;
    de.cs          = CS::CS_WRITE_2;
    de.od_index    = DO_INDEX::DO_WATCH_DOG;
    de.od_subindex = WD_SUB_INDEX::WatchdogMotorActionAfterInterruption;
    de.val         = WD_MOTOR_ACTION::StopAndEnable;
    DATA_CONV dc;
    dc.val = de;

    frame.uID         = CAN_ID::CAN_ID_SEND + drive_id;
    frame.nSendType   = 0;
    frame.bRemoteFlag = 0;
    frame.bExternFlag = 0;
    frame.nDataLen    = 8;
    for ( int i = 0; i < 8; i++ ) {
        frame.arryData[ i ] = dc.array[ i ];
    }
}

// 4、启动看门狗
void MingzhiMotor::CanDataFrameWatchDogStart( CanDataFrame& frame, uint8 drive_id ) {
    // frame.uID         = CAN_ID::CAN_ID_SEND + drive_id;
    // frame.nSendType   = 0;
    // frame.bRemoteFlag = 0;
    // frame.bExternFlag = 0;
    // frame.nDataLen    = 8;
    // for ( int i = 0; i < 8; i++ ) {
    //     frame.arryData[i] = watchdog_start[i];
    // }

    DATA_ENC de;
    de.cs          = CS::CS_WRITE_2;
    de.od_index    = DO_INDEX::DO_WATCH_DOG;
    de.od_subindex = WD_SUB_INDEX::StartOff;
    de.val         = WD_START_OFF::Start;
    DATA_CONV dc;
    dc.val = de;

    frame.uID         = CAN_ID::CAN_ID_SEND + drive_id;
    frame.nSendType   = 0;
    frame.bRemoteFlag = 0;
    frame.bExternFlag = 0;
    frame.nDataLen    = 8;
    for ( int i = 0; i < 8; i++ ) {
        frame.arryData[ i ] = dc.array[ i ];
    }
}

// 5、关闭看门狗
void MingzhiMotor::CanDataFrameWatchDogStop( CanDataFrame& frame, uint8 drive_id ) {
    // frame.uID         = CAN_ID::CAN_ID_SEND + drive_id;
    // frame.nSendType   = 0;
    // frame.bRemoteFlag = 0;
    // frame.bExternFlag = 0;
    // frame.nDataLen    = 8;
    // for ( int i = 0; i < 8; i++ ) {
    //     frame.arryData[i] = watchdog_stop[i];
    // }

    DATA_ENC de;
    de.cs          = CS::CS_WRITE_2;
    de.od_index    = DO_INDEX::DO_WATCH_DOG;
    de.od_subindex = WD_SUB_INDEX::StartOff;
    de.val         = WD_START_OFF::Off;
    DATA_CONV dc;
    dc.val = de;

    frame.uID         = CAN_ID::CAN_ID_SEND + drive_id;
    frame.nSendType   = 0;
    frame.bRemoteFlag = 0;
    frame.bExternFlag = 0;
    frame.nDataLen    = 8;
    for ( int i = 0; i < 8; i++ ) {
        frame.arryData[ i ] = dc.array[ i ];
    }
}

//设置运行参数:向0x60FF、0x6083、0x6084写入目标速度、加速度和减速度
// 伺服电机设置速度值
void MingzhiMotor::CanDataFrameSetSpeedValue( CanDataFrame& frame, uint8 drive_id, long value ) {
    uint8 sendbuffer[ 8 ];
    sendbuffer[ 0 ] = set_speed_value[ 0 ];
    sendbuffer[ 1 ] = set_speed_value[ 1 ];
    sendbuffer[ 2 ] = set_speed_value[ 2 ];
    sendbuffer[ 3 ] = set_speed_value[ 3 ];
    sendbuffer[ 4 ] = static_cast< uint8 >( value );
    sendbuffer[ 5 ] = static_cast< uint8 >( value >> 8 );
    sendbuffer[ 6 ] = static_cast< uint8 >( value >> 16 );
    sendbuffer[ 7 ] = static_cast< uint8 >( value >> 24 );

    frame.uID         = CAN_ID::CAN_ID_SEND + drive_id;
    frame.nSendType   = 0;
    frame.bRemoteFlag = 0;
    frame.bExternFlag = 0;
    frame.nDataLen    = 8;
    for ( int i = 0; i < 8; i++ ) {
        frame.arryData[ i ] = sendbuffer[ i ];
    }
}
// 伺服电机设置加速度
void MingzhiMotor::CanDataFrameSetAcce( CanDataFrame& frame, uint8 drive_id, long value ) {
    uint8 sendbuffer[ 8 ];
    sendbuffer[ 0 ] = set_drive_accec[ 0 ];
    sendbuffer[ 1 ] = set_drive_accec[ 1 ];
    sendbuffer[ 2 ] = set_drive_accec[ 2 ];
    sendbuffer[ 3 ] = set_drive_accec[ 3 ];
    sendbuffer[ 4 ] = static_cast< uint8 >( value );
    sendbuffer[ 5 ] = static_cast< uint8 >( value >> 8 );
    sendbuffer[ 6 ] = static_cast< uint8 >( value >> 16 );
    sendbuffer[ 7 ] = static_cast< uint8 >( value >> 24 );

    frame.uID         = CAN_ID::CAN_ID_SEND + drive_id;
    frame.nSendType   = 0;
    frame.bRemoteFlag = 0;
    frame.bExternFlag = 0;
    frame.nDataLen    = 8;
    for ( int i = 0; i < 8; i++ ) {
        frame.arryData[ i ] = sendbuffer[ i ];
    }
}
// 伺服电机设置减速度
void MingzhiMotor::CanDataFrameSetDece( CanDataFrame& frame, uint8 drive_id, long value ) {
    uint8 sendbuffer[ 8 ];
    sendbuffer[ 0 ] = set_drive_dece[ 0 ];
    sendbuffer[ 1 ] = set_drive_dece[ 1 ];
    sendbuffer[ 2 ] = set_drive_dece[ 2 ];
    sendbuffer[ 3 ] = set_drive_dece[ 3 ];
    sendbuffer[ 4 ] = static_cast< uint8 >( value );
    sendbuffer[ 5 ] = static_cast< uint8 >( value >> 8 );
    sendbuffer[ 6 ] = static_cast< uint8 >( value >> 16 );
    sendbuffer[ 7 ] = static_cast< uint8 >( value >> 24 );

    frame.uID         = CAN_ID::CAN_ID_SEND + drive_id;
    frame.nSendType   = 0;
    frame.bRemoteFlag = 0;
    frame.bExternFlag = 0;
    frame.nDataLen    = 8;
    for ( int i = 0; i < 8; i++ ) {
        frame.arryData[ i ] = sendbuffer[ i ];
    }
}

// 同步喂狗(心跳帧)
void MingzhiMotor::CanDataFrameSYNC( CanDataFrame& frame ) {
    frame.uID         = 0x80;
    frame.nSendType   = 0;
    frame.bRemoteFlag = 0;
    frame.bExternFlag = 0;
    frame.nDataLen    = 0;
}

//获取伺服数据的请求命令
// 获取伺服速度
void MingzhiMotor::CanDataFrameRquestMotorCurSpeed( CanDataFrame& frame, uint8 drive_id ) {
    DATA_ENC de;
    de.cs          = CS::CS_READ;
    de.od_index    = DO_INDEX::DO_READ_SPEED;
    de.od_subindex = 0;
    DATA_CONV dc;
    dc.val = de;

    frame.uID         = CAN_ID::CAN_ID_SEND + drive_id;
    frame.nSendType   = 0;
    frame.bRemoteFlag = 0;
    frame.bExternFlag = 0;
    frame.nDataLen    = 8;
    for ( int i = 0; i < 8; i++ ) {
        frame.arryData[ i ] = dc.array[ i ];
    }
}
// 获取伺服位置
void MingzhiMotor::CanDataFrameRquestMotorPosition( CanDataFrame& frame, uint8 drive_id ) {
    DATA_ENC de;
    de.cs          = CS::CS_READ;
    de.od_index    = DO_INDEX::DO_READ_POS;
    de.od_subindex = 0;
    DATA_CONV dc;
    dc.val = de;

    frame.uID         = CAN_ID::CAN_ID_SEND + drive_id;
    frame.nSendType   = 0;
    frame.bRemoteFlag = 0;
    frame.bExternFlag = 0;
    frame.nDataLen    = 8;
    for ( int i = 0; i < 8; i++ ) {
        frame.arryData[ i ] = dc.array[ i ];
    }
}
// 获取故障信息
void MingzhiMotor::CanDataFrameRquestMotorFaultCode( CanDataFrame& frame, uint8 drive_id ) {
    DATA_ENC de;
    de.cs          = CS::CS_READ;
    de.od_index    = DO_INDEX::DO_READ_ERR;
    de.od_subindex = 0;
    de.val         = 0;
    DATA_CONV dc;
    dc.val = de;

    frame.uID         = CAN_ID::CAN_ID_SEND + drive_id;
    frame.nSendType   = 0;
    frame.bRemoteFlag = 0;
    frame.bExternFlag = 0;
    frame.nDataLen    = 8;
    for ( int i = 0; i < 8; i++ ) {
        frame.arryData[ i ] = dc.array[ i ];
    }
}

// 报警清除
void MingzhiMotor::CanDataFrameAlarmClean( CanDataFrame& frame, uint8 drive_id, ALARM_CLEAR_ACTION act ) {
    DATA_ENC de;
    de.cs          = CS::CS_WRITE_1;
    de.od_index    = DO_INDEX::DO_ALARM_CLEAR;
    de.od_subindex = 0;
    de.val         = act;
    DATA_CONV dc;
    dc.val = de;

    frame.uID         = CAN_ID::CAN_ID_SEND + drive_id;
    frame.nSendType   = 0;
    frame.bRemoteFlag = 0;
    frame.bExternFlag = 0;
    frame.nDataLen    = 8;
    for ( int i = 0; i < 8; i++ ) {
        frame.arryData[ i ] = dc.array[ i ];
    }
}
